#ifndef __MOTOR_CONTROLLER_H__
#define __MOTOR_CONTROLLER_H__

#include "step_motor_user.h"
#include "base.pb.h"
#include "motor_control.pb.h"
#include "astro_coordinates/ephemeris.h"

#include <opencv2/opencv.hpp>

enum MotorId {
    MOTOR_YAW = 1,          // 方位
    MOTOR_PITCH,            // 俯仰
    MOTOR_FOCUS             // 调焦
};

enum MotorYawDirection {
    YAW_LEFT,
    YAW_RIGHT
};

enum MotorPitchDirection {
    PITCH_DOWN,
    PITCH_UP
};

enum MotorFocusDirection {
    FOCUS_FAR,
    FOCUS_NEAR
};

class MotorController {
public:
    // 步距角
    constexpr static double motor_yaw_step_angle_ = 1.8;                        
    constexpr static double motor_pitch_step_angle_ = 1.8;
    constexpr static double motor_focus_step_angle_ = 18;

    // 减速比
    constexpr static double motor_yaw_gear_ratio_ = 61.0 * 31.0 / 2 / 17;
    constexpr static double motor_pitch_gear_ratio_ = 61.0 * 31.0 / 2 / 17;
    constexpr static double motor_focus_gear_ratio_ = 360.0 / 0.5;

    // 最小频率
    constexpr static int motor_yaw_min_frequency_ = 2;
    constexpr static int motor_pitch_min_frequency_ = 2;
    constexpr static int motor_focus_min_frequency_ = 2;

    // 最大频率
    constexpr static int motor_yaw_max_frequency_ = 32000;
    constexpr static int motor_pitch_max_frequency_ = 32000;
    constexpr static int motor_focus_max_frequency_ = 32000;

    // 追星模式最大频率
    constexpr static int motor_yaw_hires_max_frequency_ = 300;
    constexpr static int motor_pitch_hires_max_frequency_ = 300;
    constexpr static int motor_focus_hires_max_frequency_ = 300;

    // 撞光电限位前的最小位置
    constexpr static double motor_yaw_min_soft_limit_ = 1.5;        // 1.5度
    constexpr static double motor_pitch_min_soft_limit_ = 1.5;      // 1.5度
    constexpr static double motor_focus_min_soft_limit_ = 0;        // 0mm

    // 撞光电限位前的最大位置
    constexpr static double motor_yaw_max_soft_limit_ = 315.5;      // 315.5度
    constexpr static double motor_pitch_max_soft_limit_ = 315.5;    // 315.5度
    constexpr static double motor_focus_max_soft_limit_ = 7;        // 7mm

    // 复位时间
    constexpr static double motor_yaw_reset_time_s_ = 15;        
    constexpr static double motor_pitch_reset_time_s_ = 15;      
    constexpr static double motor_focus_reset_time_s_ = 1;      

    // 回程（度）
    constexpr static double motor_yaw_backlash_ = 0.5;
    constexpr static double motor_pitch_backlash_ = 0.5;
    constexpr static double motor_focus_backlash_ = 0;

public:
    static void initMotorController();
    static StepMotorUser& motorYaw();
    static StepMotorUser& motorPitch();
    static StepMotorUser& motorFocus();

    int handleMessage(WsPacket& ws_packet);

    int serviceJoystick(WsPacket& ws_packet);
    int serviceJoystickFixedAngle(WsPacket& ws_packet);
    int serviceJoystickStop(WsPacket& ws_packet);
    int startDualCameraLinkage(WsPacket& ws_packet);
    int startDualCameraLinkage(cv::Point pos);

    int run(WsPacket& ws_packet);
    int runTo(WsPacket& ws_packet);
    int stop(WsPacket& ws_packet);
    int reset(WsPacket& ws_packet);
    int changeSpeed(WsPacket& ws_packet);
    int changeDirection(WsPacket& ws_packet);

private:
    StepMotorUser* getMotor(int id);

private:
    int wideViewPixelCoordToMechanicalCoord(const cv::Point& pixel_coord, HorizontalCoordinates& mechanical_coord, const HorizontalCoordinates& center_mechanical_coord);
};


#endif